/***********************************

Function: Acc_getValues.c

************************************/
#include<math.h>
#include "User_define.h"
void Acc_Filter_Value(double *,int  );
void Acc_Values_Nomalize(double *, int );
void Acc_getFinal(double *, int );
void getLPCoefficientsButterworth2Pole(const int , const double , double* const , double* const )
void filter(double* , int );
void Gyro_getValues(double *, int );
void Euler_Transform(double *, int );
void Gyro_getAngleEuler(double *, int );

void Acc_Filter_Value(double *samples, int count)
{
	double Acc[3]={0.0,0.0,0.0};
	Acc[0] = (((AdcRegs.ADCRESULT4>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_X_AXIS)*5;
	Acc[1] = (((AdcRegs.ADCRESULT6>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Y_AXIS)*5;
	Acc[2] = (((AdcRegs.ADCRESULT5>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Z_AXIS)*5;
	filter(Acc,3);

	for(int i=0;i<count;i++)
	sample[i]=Acc[i];

}

void Acc_Values_Nomalize(double *Accelerometer, int count)
{
	double Acc_N[3]={0.0,0.0,0.0};
	double norm=0.0;
	Acc_Filter_Value(Acc_N);
	norm=sqrt(Acc_N[0]*Acc_N[0]+Acc_N[1]*Acc_N[1]+Acc_N[2]*Acc_N[2]);
	Acc_N[0]=Acc_N[0]/norm;
	Acc_N[1]=Acc_N[1]/norm;
	Acc_N[2]=Acc_N[2]/norm;
	for(int i ;i<count; i++){
	Accelerometer[i]= Acc_N[i];  }

}

void Acc_getFinal(double *Acc_angle, int count)
{
	double AccAngle[2]={0.0,0.0}
	double Acc_Final[3]={0.0,0.0,0.0};
	Acc_Values_Nomalize(Acc_Final, 3);
	for(int i= 0; i<count; i++)
	{
		AccAngle[0]=asin(-Acc_Final[0];
		AccAngle[1]=asin(Acc_Final[1]/AccAngle[0];
	}
}

void getLPCoefficientsButterworth2Pole(const int samplerate, const double cutoff, double* const ax, double* const by)
{
	double PI      = 3.1415926535897932385;
	double sqrt2 = 1.4142135623730950488;

	double QcRaw  = (2 * PI * cutoff) / samplerate; // Find cutoff frequency in [0..PI]
	double QcWarp = tan(QcRaw); // Warp cutoff frequency

	double gain = 1 / (1+sqrt2/QcWarp + 2/(QcWarp*QcWarp));
	by[2] = (1 - sqrt2/QcWarp + 2/(QcWarp*QcWarp)) * gain;
	by[1] = (2 - 2 * 2/(QcWarp*QcWarp)) * gain;
	by[0] = 1;
	ax[0] = 1 * gain;
	ax[1] = 2 * gain;
	ax[2] = 1 * gain;
}

double xv[3];
double yv[3];

void filter(double* samples, int count)
{
	double ax[3];
	double by[3];

	getLPCoefficientsButterworth2Pole(100, 80, ax, by);

	for (int i=0;i<count;i++)
	{
		xv[2] = xv[1]; xv[1] = xv[0];
		xv[0] = samples[i];
		yv[2] = yv[1]; yv[1] = yv[0];

		yv[0] =   (ax[0] * xv[0] + ax[1] * xv[1] + ax[2] * xv[2]
		- by[1] * yv[0]
		- by[2] * yv[1]);

		samples[i] = yv[0];
	}
}

/*******************************************************************

GyroScope

********************************************************************/



void Gyro_getValues(double *samples, int count)
{
	double Gyro[3]={0.0,0.0,0.0};
	int j;
	double gOffsets1=0.0;
	double gOffsets2=0.0;
	double gOffsets3=0.0;
	Gyro[0] =((((AdcRegs.ADCRESULT2>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD) /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_3 pitch
	Gyro[1] =((((AdcRegs.ADCRESULT1>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI ;//theta_dot_1 roll
	Gyro[2] =((((AdcRegs.ADCRESULT3>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_2 yaw;
	

	for( j=0;j<64;j++)
	{

		gOffsets1=(gOffsets1*j+Gyro[0])/(j+1);
		gOffsets2=(gOffsets2*j+Gyro[1])/(j+1);
		gOffsets3=(gOffsets3*j+Gyro[2])/(j+1);
	}

	Gyro[0] -= gOffsets1;//pitch
	Gyro[1] -= gOffsets2;//roll
	Gyro[2] -= gOffsets3;//yaw

	for(int i=0;i<count;i++)
	sample[i]=Gyro[i];

}

void Euler_Transform(double *DT_Euler, int count)
{
	double buff_Gyro[3]={0.0,0.0,0.0};
	double buff_AngleEuler[2]={0.0,0.0};
	double Rate_Angle_Euler[2]={0.0,0.0};
	Gyro_getValues(buff_Gyro, 3);
	Get_RollPitch(buff_AngleEuler, 2);
	
	
	Rate_Angle_Euler[0]=(cos(buff_AngleEuler[1] * cos(buff_AngleEuler[0] * buff_Gyro[1] - sin(buff_AngleEuler[1]) * sin(buff_AngleEuler[0] * buff_Gyro[2])/cos(buff_AngleEuler[1]);//dt of pitch euler angle
	Rate_Angle_Euler[1]=(cos(buff_AngleEuler[0]) *  buff_Gyro[0] + sin(buff_AngleEuler[1]) * sin(buff_AngleEuler[0]) * buff_Gyro[1] + cos(buff_AngleEuler[1]) * cos(buff_AngleEuler[0]) * buff_Gyro[2])/cos(buff_AngleEuler[1])

	

	for(int i=0;i<count;i++)
	{
		DT_Euler[i]=Rate_Angle_Euler[i];
	}

}

void Gyro_getAngleEuler(double *GyroEuler, int count)
{
	double buff_AngleEuler[2]={0.0,0.0};
	double Rate_Angle[2]={0.0,0.0};
	double Angle_Gyro[2]={0.0,0.0};
	Get_RollPitch(buff_AngleEuler,2);
	Euler_Transform(Rate_Angle, 2);
	
	Angle_Gyro[0]=buff_AngleEuler[0] + Rate_Angle[0] * STEP;
	Angle_Gyro[1]=buff_AngleEuler[1] + Rate_Angle[1] * STEP;


	for(int i=0;i<count;i++)
	{
		GyroEuler[i]=Angle_Gyro[i];
	}
}

void Get_RollPitch(double *Roll_Pitch, int count)
{

	double Gyro[2]={0.0,0.0};
	double Acc[2]={0.0,0.0}
	double Angle_Robot[2]={0.0,0.0};

	Gyro_getAngleEuler(Gyro, 2);
	Acc_getFinal(Acc, 2);


	Angle_Robot[0]=Gyro[0] * 0.98 + Acc[0] * 0.02;
	Angle_Robot[1]=Gyro[1] * 0.98 + Acc[1] * 0.02;

	for( int i=0; i<count; i++)
	{
		Roll_Pitch[i]=Angle_Robot[i];

	}
}
	

